﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static System.Math;

namespace Demo_2D
{
    /// <summary>
    /// 坐标结构体定义
    /// </summary>
    struct PointC
    {
        public int x;
        public int y;
        public int z;
    }

    /// <summary>
    /// 标签与基站距离值
    /// </summary>
    struct AllDis
    {
        public int d_1;
        public int d_2;
        public int d_3;
        public int d_4;
    }

    /// <summary>
    /// 坐标类定义
    /// </summary>
    class Coordinate
    {

        /// <summary>
        /// 自定义 POW函数用以转换数据类型
        /// </summary>
        /// <returns></returns>
        int MyPow(int a, int b)
        {
            int ret = 0;
            for(int i = 0; i < b; i++)
            {
                a *= a;
            }
            return ret;
        }
        
        /// <summary>
        /// 根据三个基站坐标标签距离值计算 标签 2D 坐标
        /// </summary>
        /// <param name="ant1"></param>
        /// <param name="ant2"></param>
        /// <param name="ant3"></param>
        /// <param name="dis"></param>
        /// <returns></returns>
        PointC CalcTagPoint(PointC ant1, PointC ant2, PointC ant3, AllDis dis)
        {
            PointC t_p = new PointC();

            int r1 = dis.d_1;
            int r2 = dis.d_2;
            int r3 = dis.d_3;

            int x1 = ant1.x;
            int x2 = ant2.x;
            int x3 = ant3.x;

            int y1 = ant1.y;
            int y2 = ant2.y;
            int y3 = ant3.y;

            int t = 2;
            t_p.x = (((y3 - y2) * MyPow(x1, t) + MyPow(r1, t) * y2 - MyPow(r1, t) * y3 - MyPow(r2, t) * y1 + MyPow(r2, t) * y3 + MyPow(r3, t) * y1 - MyPow(r3, t) * y2 + MyPow(x2, t) * y1 - MyPow(x2, t) * y3 - MyPow(x3, t) * y1 + MyPow(x3, t) * y2 - MyPow(y1, t) * y2 + MyPow(y1, t) * y3 + y1 * MyPow(y2, t) - y1 * MyPow(y3, t) - MyPow(y2, t) * y3 + y2 * MyPow(y3, t)) / ((t * y3 - t * y2) * x1 + t * x2 * y1 - t * x3 * y1 - t * x2 * y3 + t * x3 * y2));

            t_p.y = ((x3 - x2) * MyPow(x1, t) + (-MyPow(r2, t) + MyPow(r3, t) + MyPow(x2, t) - MyPow(x3, t) + MyPow(y2, t) - MyPow(y3, t)) * x1 + MyPow(r1, t) * x2 - MyPow(r1, t) * x3 + MyPow(r2, t) * x3 - MyPow(r3, t) * x2 - MyPow(x2, t) * x3 + x2 * MyPow(x3, t) - x2 * MyPow(y1, t) + x2 * MyPow(y3, t) + x3 * MyPow(y1, t) - x3 * MyPow(y2, t)) / ((t * y2 - t * y3) * x1 + t * x3 * y1 - t * x2 * y1 + t * x2 * y3 - t * x3 * y2);
            return t_p;
        }
    }
}
